//
// Created by bobin on 17-11-15.
//

#ifndef PNPL_POSEOPTIMIZE_H
#define PNPL_POSEOPTIMIZE_H

#include "G2OTypes.h"
#include "opencv2/core/core.hpp"
#include "vector"


class Optimize {
public:
    Optimize(const std::vector<Eigen::Vector3d> &_x3dPoints, const std::vector<Eigen::Vector2d> &_x2dPoints,
             const Eigen::Matrix3d &_K, const std::vector<int> &_pointCate,
             const std::vector<int> &_pointLineIndex,
             const std::vector<int> &_pointPlaneIndex,
             const std::vector<g2o::Vector6D> &_lines,
             const std::vector<Eigen::Vector4d> &_planes);

    static void PnPL_plucker(const std::vector<cv::Point3f> &pts3d, const std::vector<cv::Point2f> &pts2d,
                             const std::vector<cv::Vec6f> &lns3d, const std::vector<cv::Vec4f> &lns2d,
                             const cv::Mat &K, cv::Mat &R, cv::Mat &t);

    static void PnPL(const std::vector<cv::Point3f> &pts3d, const std::vector<cv::Point2f> &pts2d,
                     const std::vector<cv::Vec6f> &lns3d, const std::vector<cv::Vec4f> &lns2d,
                     const cv::Mat &K, cv::Mat &R, cv::Mat &t, bool line_constraint = false);

    void bundleAdjust(Eigen::Matrix3d &R, Eigen::Vector3d &tb, bool useStruct = false);

    enum {
        SpacePoint,
        LinePoint,
        PlanePoint
    } PointCategory;

private:
    std::vector<Eigen::Vector3d> m3dPoints;
    std::vector<Eigen::Vector2d> m2dPoints;
    std::vector<Eigen::Vector4d> mPlanes;

    std::vector<g2o::Vector6d> mLines;
    std::vector<int> mPointCate;
    std::vector<int> mPointLineIndex;
    std::vector<int> mPointPlaneIndex;
    Eigen::Matrix3d K;
};


#endif //PNPL_POSEOPTIMIZE_H
